#include <filesystem>
#include <memory>
#include <string>

#include "rtdev_setting_cmd.h"
#include "log/zuclog.h"
#include "rtdev/rtdev.h"
#include "rtdev/rs485msgque.h"
#include "rtdev/safetyboard/scbitf.h"
#include "rtbus/rtbus.h"
#include "motion/mot_priv.h"
#include "errcode.h"
#include "motion/semintf.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/axisgroup_set.h"
#include "axisgroup/robot_mgr.h"
#include "config.h"
#include "jkutil/iniparser.h"
#include "safety/io/safeio.h"

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD", fmt, ##__VA_ARGS__)

static int newConfigTemp = 255;

int CmdShutDown::process() { return rtdev::RtDevMgr::instance().get_scb()->cab()->do_poweroff_ipc(); }

int CmdGetRs485Message::process()
{
    MOTCMD_INFO("ZUCMOT_GET_RS485_MESSAGE\n");
    zucmotStatus->rs485_chn = chn_;
    zucmotStatus->rs485_len = len_;
    // MOTCMD_INFO("RsMsgEnqueueSize = %d\n", RsMsgQueSize(zucmotStatus->rs485_chn));
    if (zucmotStatus->rs485_len > RsMsgQueSize(zucmotStatus->rs485_chn))
    {
        zucmotStatus->rs485_len = RsMsgQueGetAll(zucmotStatus->rs485_chn, zucmotStatus->RS485_msg);
    }
    else
    {
        zucmotStatus->rs485_len = RsMsgDequeueAlot(zucmotStatus->rs485_chn, zucmotStatus->RS485_msg, zucmotStatus->rs485_len);
    }
    // MOTCMD_INFO("COMMAND get msg %d\n", zucmotStatus->rs485_len);
    // for (int i = 0; i < zucmotStatus->rs485_len; i++) { MOTCMD_INFO("%02x \n", zucmotStatus->RS485_msg[i]); }
    return 0;
}

int CmdSetTioRs485SigProgStat::process()
{
    MOTCMD_INFO("ZUCMOT_SET_TIO_RS485_SIG_PROG_STAT\n");
    if (tio_gripper_set_signal_prog_stat(sig_name_, prog_stat_))
    {
        MOTCMD_INFO("No signal named %s\n", sig_name_);
        return -1;
    }
    GripperStateUnit* grrpperState = tio_gripper_list_signal();
    for (int i = 0; i < MAX_TIO_SIG_NUM; i++) { zucmotStatus->gripperState[i] = grrpperState[i]; }
    return 0;
}

int CmdSetmodeTorqsensor::process()
{
    MOTCMD_INFO("ZUCMOT_SETMODE_TORQSENSOR\n");
    if (GET_MOTION_ADMITTANCE_FLAG())
    {
        reportError(ADMIT_CANNOT_SET_MODE_IN_OPERATION, ("Cannot switch senor mode when in admittance control"));
    }
    else
    {
        int sem_id = sem_get(SENSOR_SEM_KEY);
        if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND && mode_ == 1)
        {
            if (zucmotConfig->tioConfig.rsChnCfg[0].mode != 2 && zucmotConfig->tioConfig.rsChnCfg[1].mode != 2)
            {
                reportError(TIO_NO_CHN_FOR_TORQSNSOR, "no tio rs485 channel is configured for torque sensor");
                return 0;
            }
            else if (zucmot_hal_data->zuc_enable_cmd == 0)
            {
                reportError(ROBOT_POWERED_OFF, ("please power on the sensor first"));
                return 0;
            }
        }
        if (zucmotSensorStruct->sensorConfig.commType != 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND && mode_ == 1 &&
            zucmotSensorStruct->sensorStatus.status != 2)
        {
            // 配置错误
            if (sem_p(sem_id) == 0)
            {
                zucmotSensorStruct->sensorStatus.status = 2;
                zucmotSensorStruct->sensorStatus.errCode = 0x0001;
                sem_v(sem_id);
            }
            return 0;
        }
        MOTCMD_INFO("zucmotSensorStruct->sensorConfig.mode = %d, zucmotSensorStruct->sensorConfig.commType = %d\n",
                    zucmotSensorStruct->sensorConfig.mode,
                    zucmotSensorStruct->sensorConfig.commType);
        if (sem_p(sem_id) == 0)
        {
            zucmotSensorStruct->sensorConfig.mode = mode_;
            zucmotSensorStruct->sensorStatus.errCode = 0;
            zucmotSensorStruct->sensorStatus.status = 0;
            sem_v(sem_id);
        }
    }
    // 8 型传感器为预留传感器，暂不支持
    if (zucmotSensorStruct->sensorConfig.torqsensorBrand == 8 && mode_ == 1)
    {
        int sem_id = sem_get(SENSOR_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            zucmotSensorStruct->sensorStatus.status = 2;
            zucmotSensorStruct->sensorStatus.errCode = 0x0001;
            sem_v(sem_id);
        }
    }
    return 0;
}

int CmdSetbrandTorqsensor::process()
{
    if (GET_MOTION_ADMITTANCE_FLAG())
    {
        reportError(ADMIT_CANNOT_SET_MODE_IN_OPERATION, ("Cannot switch senor brand when the robot is  in admittance control"));
    }
    else
    {
        int sem_id = sem_get(SENSOR_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            zucmotSensorStruct->sensorConfig.torqsensorBrand = torqsensorBrand_;
            sem_v(sem_id);
        }
    }
    MOTCMD_INFO("ZUCMOT_SETBRAND_TORQSENSOR %d\n", torqsensorBrand_);
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        zucmotSensorStruct->sensorStatus.errCode = 0;
        zucmotSensorStruct->sensorConfig.mode = 0;
        zucmotSensorStruct->sensorStatus.status = 0;
        sem_v(sem_id);
    }
    return 0;
}

int CmdSetcommTorqsensor::process()
{
    MOTCMD_INFO("CmdSetcommTorqsensor %d", type_);
    int sem_id = sem_get(SENSOR_SEM_KEY);
    zucmot_config_change();
    // if (sem_p(sem_id) == 0)
    // {
    //     zucmotSensorStruct->sensorConfig.commType = type_;
    //     MOTCMD_INFO("sensor comm type = %d\n", zucmotSensorStruct->sensorConfig.commType);
    //     if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND)
    //     {
    //         if (zucmotConfig->tioConfig.rsChnCfg[0].mode == 2)
    //         {
    //             zucmotSensorStruct->sensorConfig.commCfg.rtuComm = zucmotConfig->tioConfig.rsChnCfg[0].rtuComm;
    //         }
    //         else if (zucmotConfig->tioConfig.rsChnCfg[1].mode == 2)
    //         {
    //             zucmotSensorStruct->sensorConfig.commCfg.rtuComm = zucmotConfig->tioConfig.rsChnCfg[1].rtuComm;
    //         }
    //     }
    //     else
    //     {
    //         zucmotSensorStruct->sensorConfig.commCfg = commCfg_;
    //         MOTCMD_INFO(
    //             "sensor comm: %s, port: %d\n", zucmotSensorStruct->sensorConfig.commCfg.tcpComm.ipaddr, zucmotSensorStruct->sensorConfig.commCfg.tcpComm.port);
    //     }
    //     sem_v(sem_id);
    // }
    return 0;
}

int CmdTioSetRs485ChnMode::process()
{
    MOTCMD_INFO("CmdTioSetRs485ChnMode %d, %d", chnId_, mode_);
    zucmot_config_change();
    if (zucmotStatus->powered_on && (zucmotStatus->tio_status.tio_version & 0xFF00) < 0x0300)
    {
        reportError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function: %d", ZUCMOT_TIO_SET_RS485_CHN_MODE);
        zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
        return -1;
    }
    if (mode_ == 0 || mode_ == 1)
    {  // modbus RTU or raw rs485
        if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND &&
            zucmotSensorStruct->sensorStatus.status == 1)
        {
            if ((chnId_ == 0 && zucmotConfig->tioConfig.rsChnCfg[0].mode == 2) || (chnId_ == 1 && zucmotConfig->tioConfig.rsChnCfg[1].mode == 2))
            {
                reportError(TIO_ERR_SET_RSCOMM_WHEN_SNSOR_RUNNING, "failed to change tio rs channle settings since a torque sensor is running");
                zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
                return -1;
            }
        }
        if (mode_ != 0)
        {  // clear signals of this channel
            tio_gripper_clr_signal(chnId_);
        }
        zucmotConfig->tioConfig.rsChnCfg[chnId_].mode = mode_;
        if (zucmotStatus->powered_on)
        {
            set_tio_rs485_chn_mode(chnId_, mode_);
        }
    }
    else if (mode_ == 2)
    {  // torque sensor
        if ((chnId_ == 0 && zucmotConfig->tioConfig.rsChnCfg[1].mode == 2) || (chnId_ == 1 && zucmotConfig->tioConfig.rsChnCfg[0].mode == 2))
        {
            reportError(TIO_MAX_ONE_CHN_FOR_TORQSNSOR, "only one rs485 channel at most can be configured for torque sensor");
            zucmotStatus->commandStatus = ZUCMOT_COMMAND_INVALID_PARAMS;
            return -1;
        }
        tio_gripper_clr_signal(chnId_);
        zucmotConfig->tioConfig.rsChnCfg[chnId_].mode = mode_;
        if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND)
        {
            // zucmotSensorStruct->sensorConfig.commCfg.rtuComm = zucmotConfig->tioConfig.rsChnCfg[chnId_].rtuComm;
        }
        if (zucmotStatus->powered_on)
        {
            set_tio_rs485_chn_mode(chnId_, mode_);
        }
    }
    // MOTCMD_INFO("chanle: %d, mode: %d\n", mode_, mode_);
    // update torque sensor config register
    if (zucmotStatus->powered_on)
    {
        if (zucmotConfig->tioConfig.rsChnCfg[0].mode == 2)
        {
            tio_set_rs485_chn_func(1);
        }
        else if (zucmotConfig->tioConfig.rsChnCfg[1].mode == 2)
        {
            tio_set_rs485_chn_func(2);
        }
        else
        {
            tio_set_rs485_chn_func(0);
        }
    }
    zucmot_config_change();
    return 0;
}

int CmdTioSetRs485ChnComm::process()
{
    MOTCMD_INFO("CmdTioSetRs485ChnComm %d", chnId_);
    zucmot_config_change();
    if (zucmotStatus->powered_on && (zucmotStatus->tio_status.tio_version & 0xFF00) < 0x0300)
    {
        reportError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function: %d", ZUCMOT_TIO_SET_RS485_CHN_COMM);
        return -1;
    }
    ModRtuComm rtuComm = rtuComm_;
    zucmotConfig->tioConfig.rsChnCfg[chnId_].rtuComm = rtuComm;
    if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND &&
        zucmotConfig->tioConfig.rsChnCfg[chnId_].mode == 2)
    {
        // zucmotSensorStruct->sensorConfig.commCfg.rtuComm = zucmotConfig->tioConfig.rsChnCfg[chnId_].rtuComm;
    }
    if (zucmotStatus->powered_on)
    {
        set_tio_rs485_chn_comm(chnId_, rtuComm.baudrate, rtuComm.databits, rtuComm.stopbits, rtuComm.parity);
    }
    zucmot_config_change();
    return 0;
}

int CmdTioSetPinMode::process()
{
    MOTCMD_INFO("CmdTioSetPinMode %d", mode_);
    zucmot_config_change();
    if (zucmotStatus->powered_on && (zucmotStatus->tio_status.tio_version & 0xFF00) < 0x0300)
    {
        reportError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function: %d", ZUCMOT_TIO_SET_PIN_MODE);
        return -1;
    }
    if (type_ == 0)
    {  // 2 dins
        zucmotConfig->tioConfig.pinCfg.diCfg = mode_;
        if (zucmotStatus->powered_on)
        {
            tio_setup_di_pin(mode_);
        }
    }
    else if (type_ == 1)
    {  // 2 douts
        if (mode_ != 0xFF && zucmotConfig->tioConfig.pinCfg.doCfg == 0xFF)
        {
            if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND &&
                zucmotSensorStruct->sensorStatus.status == 1)
            {
                reportError(TIO_ERR_SET_RSCOMM_WHEN_SNSOR_RUNNING, "failed to change tio rs channle settings since a torque sensor is running");
                return 0;
            }
            tio_gripper_clr_signal(0);  // clear signals of this channel
        }
        zucmotConfig->tioConfig.pinCfg.doCfg = mode_;
        zucmotConfig->tioConfig.rsChnCfg[0].mode = 1;  // reset RS channel mode
        if (zucmotStatus->powered_on)
        {
            tio_setup_do_pin(mode_);
        }
    }
    else if (type_ == 2)
    {  // 2 ain
        if (mode_ == 0 && zucmotConfig->tioConfig.pinCfg.aiCfg == 1)
        {
            if (zucmotSensorStruct->sensorConfig.commType == 1 && zucmotSensorStruct->sensorConfig.torqsensorBrand == TIO_SENSOR_BRAND &&
                zucmotSensorStruct->sensorStatus.status == 1)
            {
                reportError(TIO_ERR_SET_RSCOMM_WHEN_SNSOR_RUNNING, "failed to change tio rs channle settings since a torque sensor is running");
                return 0;
            }
            tio_gripper_clr_signal(1);  // from rs485 to AI pins
        }
        zucmotConfig->tioConfig.pinCfg.aiCfg = mode_;
        zucmotConfig->tioConfig.rsChnCfg[1].mode = 1;  // reset RS channel mode
        if (zucmotStatus->powered_on)
        {
            tio_setup_ai_pin(mode_);
        }
    }
    zucmot_config_change();
    return 0;
}

int CmdTioSetVoutEna::process()
{
    MOTCMD_INFO("CmdTioSetVoutEna %d", enable_);
    zucmot_config_change();
    if (zucmotStatus->powered_on && (zucmotStatus->tio_status.tio_version & 0xFF00) < 0x0300)
    {
        reportError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function: %d", ZUCMOT_TIO_SET_VOUT_ENA);
        return 0;
    }
    zucmotConfig->tioConfig.voutEna = enable_;
    if (zucmotStatus->powered_on)
    {
        tio_set_vout_enable(zucmotConfig->tioConfig.voutEna);
    }
    zucmot_config_change();
    return 0;
}

int CmdTioSetVoutVol::process()
{
    MOTCMD_INFO("CmdTioSetVoutVol %d", voltage_);
    zucmot_config_change();
    if (zucmotStatus->powered_on && (zucmotStatus->tio_status.tio_version & 0xFF00) < 0x0300)
    {
        reportError(FUNCTION_NOT_SUPPORTED_BY_HARDWARE, "hardware doesn't support this function: %d", ZUCMOT_TIO_SET_VOUT_VOL);
        return 0;
    }
    zucmotConfig->tioConfig.voutVol = voltage_;
    if (zucmotStatus->powered_on)
    {
        tio_set_vout_voltage(zucmotConfig->tioConfig.voutVol);
    }
    zucmot_config_change();
    return 0;
}

int CmdSetCabvoltage::process()
{
    auto cab = rtdev::RtDevMgr::instance().get_scb()->cab();
    MOTCMD_INFO("set cab voltage from %d to %d\n", cab->get_power_supply(), cabVoltage_);
    cab->set_power_supply(cabVoltage_);
    return 0;
}

int CmdSetBrStartVoltage::process()
{
    auto cab = rtdev::RtDevMgr::instance().get_scb()->cab();
    cab->set_stuck_voltage(scb_br_start_voltage_);
    MOTCMD_INFO("zucmot set return 0ing resister start voltage = %d\n", scb_br_start_voltage_);
    zucmotStatus->set_scb_br_start_vol_flag = 1;
    zucmotStatus->is_set_scb_br_start_vol_flag = 0;
    zucmotStatus->after_set_scb_br_start_vol_flag = 0;
    return 0;
}

int CmdServoUpgrade::process()
{
    MOTCMD_INFO("CmdServoUpgrade");
    // zucmotConfig->servoUpGrade = 1;

    // TODO:本体升级处理
    // rtdev::RtDevMgr::instance().get_scb()->cab()->disable_robot_commu(1);
    // rtbus::exit();
    // zucmot_config_change();
    return -1;
}

int CmdSetFreeButtonOpt::process()
{
    MOTCMD_INFO("ZUCMOT_SET_FREE_BUTTON_OPT: %d\n", option_);
    zucmotStatus->freeButtonOpt = option_;
    return 0;
}

int CmdSetPointButtonOpt::process()
{
    MOTCMD_INFO("ZUCMOT_SET_POINT_BUTTON_OPT: %d\n", option_);
    zucmotStatus->pointButtonOpt = option_;
    return 0;
}

int CmdSetFreeProgButtonOpt::process()
{
    MOTCMD_INFO("ZUCMOT_SET_FREE_PROG_BUTTON_OPT: %d, %d\n", freeOpt_, progOpt_);
    zucmotStatus->secondfreeButtonOpt = freeOpt_;
    zucmotStatus->progButtonOpt = progOpt_;
    return 0;
}

int CmdSetupIoChannel::process()
{
    MOTCMD_INFO("CmdSetupIoChannel, %d, %d, %d", id_, ioType_, channelsetting_);
    if (ioType_ == 0)
    {
        static unsigned char newConfig = 170;
        unsigned char tarSetting = 0x0A;
        switch (channelsetting_)
        {
        case 1:
            tarSetting = 0x0A;
            return 0;
        case 2:
            tarSetting = 0x09;
            return 0;
        case 3:
            tarSetting = 0x06;
            return 0;
        case 4:
            tarSetting = 0x05;
            return 0;
        default:
            return 0;
        }

        if (id_ == 0)
        {
            newConfig = (tarSetting << 4) + (newConfig & 0xF);
        }
        else if (id_ == 1)
        {
            newConfig = (newConfig & 0xF0) + (tarSetting & 0xF);
        }
        rtdev::RtDevMgr::instance().get_scb()->io()->config_aio_type(newConfig);
        // set_scb_ao_type(newConfig);
    }
    else if (ioType_ == 1 && zucmotConfig->cab_type == 3)
    {  // only minicab support digital channel settings
        // fixme  newConfig value is not right
        if (id_ < 0 || id_ >= 7)
            return 0;
        if (channelsetting_)
            newConfigTemp |= (1 << id_);
        else
            newConfigTemp &= ~(1 << id_);
        rtdev::RtDevMgr::instance().get_scb()->io()->config_dio_type(newConfigTemp);
        // set_pscb_user_di_type(newConfigTemp);
    }
    return 0;
}

int CmdSetCabinetId::process()
{
    rtdev::RtDevMgr::instance().get_scb()->cab()->set_id(cab_id_);
    // set_scb_cab_id(cab_id_);
    return 0;
}

int CmdRobotPowerOn::active_callback()
{
    if (!rtdev::RtDevMgr::instance().get_scb()->is_alive())
    {
        return -1;
    }
    bool all_powered = true;
    for (size_t i = 0; i < mot::al::MotionProxy::instance().robot_cnt(); i++)
    {
        auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
        if (!status.is_power_on && !status.is_powering)
        {
            all_powered = false;
        }
    }
    if (all_powered)
    {
        return -1;
    }
    if (safety::SafeIOMgr::instance().is_estop())
    {
        zucOperatorWarning(INFO_EMERGENCY_STOP_OCCURS_CANNOT_POWER_ON, "When an emergency stop occurs, the power-on operation cannot be performed.");
        return -1;
    }

    // if (zucStatus->cab_type == 1)  //v1.7.1后不支持cabv1
    // {
    //     zucOperatorError(UNSUPPORTED_CABINET, "unsupport cabv1#{\"fixed_key\":[\"CabV1\"]}");
    //     return -1;
    // }
    // if (!zucStatus->motion.powered_on)
    // {
    //     if (true == zucStatus->motion.func_io_req.power_off_req)
    //     {  // funcio power_off_req  is  true.
    //         zucOperatorError(ERR_FDI_POWER_ON_LOCKED, "robot power on  function is locked by power-off-request of funcIO.");
    //         return -1;
    //     }
    //     zucMotionEstopOff();
    //     if (zucStatus->motion.category_one_stop_program_pause == 0)
    //     {
    //         zucTaskAbort();
    //         zucAbortCleanup(ZUC_ABORT_TASK_STATE_POWERD_ON);
    //         zucTaskPlanSynch();
    //     }
    // }
    // else
    // {
    //     zucOperatorError(INFO_ROBOT_HAS_BEEN_POWERED_ON, "The robot has been powered on .");
    //     return -1;
    // }

    return 0;
}

int CmdRobotPowerOn::process()
{
    axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->do_power(true);  // 切换至booting状态
    return rtdev::RtDevMgr::instance().get_scb()->rob_power()->set_power(true, group_id_);
}

int CmdPowerOffRobot::active_callback()
{
    bool enabled = false;
    for (size_t i = 0; i < mot::al::MotionProxy::instance().robot_cnt(); i++)
    {
        auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
        if (status.is_enable || status.is_enabling)
        {
            enabled = true;
        }
    }
    if (enabled)
    {
        return -1;
    }
    // //servo off first
    // if (zucStatus->motion.is_enabled)
    // {
    //     zucOperatorError(SERVO_OFF_BEFORE_POWER_OFF, "%s", "please servo off first before powering off the robot");
    //     return -1;
    // }
    // if (zucStatus->motion.category_one_stop_program_pause == 1)
    // {
    //     zuc_mot_robot_poweroff();
    //     for (t = 0; t < zucStatus->motion.traj.joints; t++) { zucJointDisable(t); }
    //     zucTrajDisable();
    // }
    // else
    // {
    //     zucMotionAbort();
    //     zuc_mot_robot_poweroff();
    //     for (t = 0; t < zucStatus->motion.traj.joints; t++) { zucJointDisable(t); }
    //     zucTrajDisable();
    //     zucTaskAbort();
    //     zucJointUnhome(-2);  // only those joints which are volatile_home
    //     zucAbortCleanup(ZUC_ABORT_TASK_STATE_POWERED_OFF);
    //     zucTaskPlanSynch();
    // }
    // zucMotionExitProtect(0);  // 本体下电后清除保护性停止标记,以便重新上电、使能
    // break;

    return 0;
}

int CmdPowerOffRobot::process()
{
    auto rob = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    rob->do_power(false);  // 切换至booting状态
    auto config = rob->get_robot_active_config();
    MOTCMD_DEBUG("Try to deactive bus %s", config.robot_axis_cfg.bus_chn.c_str());
    //FIXME：换成非阻塞的
    auto bus = rtbus::BusMgr::get()->bus(config.robot_axis_cfg.bus_chn);
    if (!bus.get())
    {
        MOTCMD_DEBUG("bus %s not found", config.robot_axis_cfg.bus_chn.c_str());
        return -1;
    }
    bus->deactive();
    return rtdev::RtDevMgr::instance().get_scb()->rob_power()->set_power(false, group_id_);
}

int CmdRobotAxisBoot::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto config = robot->get_robot_active_config();
    //获取机器总线和轴配置

    // for (int i = axis_start_id_; i < axis_start_id_ + axis_num_; i++)
    // {
    //     auto a = axis::AxisMgr::get().axis(i);
    //     if (!a)
    //     {
    //         break;
    //     }
    //     a->boot_init(busname_, i);  // 重新bind bus
    //     // axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->link_joint(a.get());
    // }

    //上面做完了再激活总线
    MOTCMD_DEBUG("Try to active bus %s", config.robot_axis_cfg.bus_chn.c_str());
    auto bus = rtbus::BusMgr::get()->bus(config.robot_axis_cfg.bus_chn);
    if (!bus.get())
    {
        MOTCMD_DEBUG("bus %s not found", config.robot_axis_cfg.bus_chn.c_str());
        return -1;
    }
    bus->active();
    return 0;
}

int CmdRobotBoot::process() { return 0; }
